package org.team751.dev;

import edu.wpi.first.wpilibj.ADXL345_I2C;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Timer;
import java.util.TimerTask;
import org.team751.util.WPILibTimer;

/**
 * Handles inertial navigation using an ADXL345 accelerometer and a gyroscope sensor
 * @author Sam Crow
 */

public class InertialNavigator {

    /** The number of milliseconds to wait between calculations */
    private static final long delay = 10;
    
    private Timer timer;
    private INavTask task;

    public InertialNavigator(ADXL345_I2C accel, Gyro gyro){
    	task = new INavTask(accel, gyro);
        timer = new Timer();
        timer.schedule(task, 0, delay);//start calculating things
    }

    /**
     * This is the actual TimerTask that does the processing for inertial navigation
     * @author Sam Crow
     */
    private class INavTask extends TimerTask {
		private ADXL345_I2C accel;
        private Gyro gyro;
        
        /** The acceleration value from the accelerometer that is considered to mean zero acceleration in the X axis */
        private double refX;
        /** The acceleration value from the accelerometer that is considered to mean zero acceleration in the Y axis */
        private double refY;
        
        /** The speed along the Y axis, in meters/second/second, as of the most recent calculation */
        private double ySpeed = 0;
        
        private double yDistance = 0;
        
        /** The time, in seconds since robot startup, of the last calculation. Used to find the time between calculations */
        private double calcTimestamp = WPILibTimer.getFPGATimestamp();

        /**
         * Constructor
         * @param accel The accelerometer to use to access acceleration data
         * @param gyro The gyroscope to use to access heading data
         */
        public INavTask(ADXL345_I2C accel, Gyro gyro){
            this.accel = accel;
            this.gyro = gyro;
            
            refX = accel.getAcceleration(ADXL345_I2C.Axes.kX);
            refY = accel.getAcceleration(ADXL345_I2C.Axes.kY);
            System.out.println("Inertial navigation: Setting up. Using x="+refX+", y="+refY+" as reference values for zero acceleration.");
        }

        public void run() {
            //Try only the Y axis for now
            double acceleration = getCorrectedY();
            //convert from Gs to meters/second/second
            acceleration *= 9.8;
            SmartDashboard.putDouble("Y acceleration in m/s/s", acceleration);
            
            double time = WPILibTimer.getFPGATimestamp() - calcTimestamp;
            SmartDashboard.putDouble("Time since last calculation", time);
            
            //Get the change in speed since the last calculation by multiplying the acceleration by the time
            double speedChange = acceleration * time;
            SmartDashboard.putDouble("Speed change", speedChange);
            
            ySpeed += speedChange;
            SmartDashboard.putDouble("Speed", ySpeed);
            
            //Now that we have the new speed, calculate the change in distance
            double distanceChange = ySpeed * time;
            SmartDashboard.putDouble("Distance change", distanceChange);
            
            yDistance += distanceChange;
            SmartDashboard.putDouble("Y Distance", yDistance);
            
            calcTimestamp = WPILibTimer.getFPGATimestamp();
        }
        
        //utility methods related to accelerometer value correction
        /** Get the value of the accelerometer in the X axis, relative to the reference acceleration */
	private double getCorrectedX(){
        	double acceleration = accel.getAcceleration(ADXL345_I2C.Axes.kX) - refX;
                SmartDashboard.putDouble("Corrected X acceleration", acceleration);
                return acceleration;
        }
	/** Get the value of the accelerometer in the X axis, relative to the reference acceleration */
	private double getCorrectedY(){
		double acceleration =  accel.getAcceleration(ADXL345_I2C.Axes.kY) - refY;
                SmartDashboard.putDouble("Corrected Y acceleration", acceleration);
                return acceleration;
	}
    }
}
